#include "lenstrayloader.h"

LensTrayLoader::LensTrayLoader(LensTrayModuleConfig *ltConfig,
                               LensTrayModuleState *ltState,
                               LensTrayMagazine *ltMag,
                               LensTrayBuffer *ltBuffer,
                               TrayConfig *lensTrayConfig,
                               LensTrayMap *lensTrayMap,
                               QObject *parent)
    : Worker(WorkerName::LensTrayLoader, parent),
      ltConfig(ltConfig),
      ltState(ltState),
      ltMag(ltMag),
      ltBuffer(ltBuffer),
      lensTrayConfig(lensTrayConfig),
      lensTrayMap(lensTrayMap)
{
    resetRowColumnIndex();
    lastLensIndex.setX(-1);
    lastLensIndex.setY(-1);
}

void LensTrayLoader::setBHConfig(BonderHeadConfig *bhConfig)
{
    this->bhConfig = bhConfig;
}

PrOffset LensTrayLoader::getLensPrOffset(int index)
{
    checkIsInit();
    PrOffset prOffset;
    if (!ltLensLocation->performPR(prOffset, 0, index))
    {
        throw SilicolAbort(tr("Lens pr failed!"));
    }
    correctPrOffset(prOffset);
    return prOffset;
}

PrOffset LensTrayLoader::getLTVacancyPrOffset(int index)
{
    checkIsInit();
    PrOffset prOffset;
    if (!ltVacancyLocation->performPR(prOffset, 0, index))
    {
        throw SilicolAbort(tr("Lens tray vacancy pr failed!"));
    }
    correctPrOffset(prOffset);
    return prOffset;
}

void LensTrayLoader::correctPrOffset(PrOffset &prOffset)
{
    if (!qFuzzyCompare(bhConfig->ltCameraPrPosXShift(), 0))
    {
        ltLoaderTrackX->axis()->relMove(bhConfig->ltCameraPrPosXShift() * -1);
        prOffset.X -= bhConfig->ltCameraPrPosXShift();
        prOffset.O_X -= bhConfig->ltCameraPrPosXShift();
    }
}

bool LensTrayLoader::gotLens() const
{
    return m_gotLens;
}

void LensTrayLoader::resetGotLens()
{
    m_gotLens = false;
}

void LensTrayLoader::setLensState(QPoint index, LensTrayMaterialData::MaterialState state)
{
    auto data = lensTrayMap->getData<LensTrayMaterialData>(index.x(), index.y());
    data->setMaterialState(state);
}

PrOffset LensTrayLoader::getLastLensOffset() const
{
    return lastLensOffset;
}

TrayMap *LensTrayLoader::getTrayMap() const
{
    return lensTrayMap;
}

QPoint LensTrayLoader::getLastLensIndex() const
{
    return lastLensIndex;
}

void LensTrayLoader::initEventHandle()
{
    ltLoaderBelt = MotionElementContainer::getIns()->getItem<SCAxis>(AxisName::LTLoaderBelt);
    ltLoaderBeltZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::LTLoaderBeltZ);
    ltLoaderTrackX = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::LTLoaderTrackX);
    ltBufferBeltZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::LTBufferBeltZ);
    ltlClamp = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::LTLClamp);
    ltlUnloadKicker = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::LTLUnloadKicker);
    ltlLeftDam = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::LTLLeftDam);
    ltLoaderStageSensor = MotionElementContainer::getIns()->getItem<SCDI>(DiName::LTLoaderStageSNR);
    ltLoaderLeftSensor = MotionElementContainer::getIns()->getItem<SCDI>(DiName::LTLoaderLeftSNR);

    beltRunDir = SCAxis::DirEnumInfo().nameToEnum(ltConfig->ltLoaderBeltRunDir());

    ltLensLocation = VisionManager::getIns().getVisionLocation(VLName::LTLensLocation);
    ltVacancyLocation = VisionManager::getIns().getVisionLocation(VLName::LTVacancyLocation);
}

void LensTrayLoader::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event)
    resetRowColumnIndex();
    hasSynchronizeHasTray = false;
    isLtBufferStopped = false;
    pushEvent(WorkerEvent("kickerAndClampToReady"));
    pushEvent(WorkerEvent("loadTray"));
}

void LensTrayLoader::allWorkerStopped()
{
    isLtBufferStopped = false;
}

void LensTrayLoader::purgeEventHandle()
{
    purgeEventHandled();
}

void LensTrayLoader::resetEventHandle()
{
    isLtBufferStopped = false;
}

void LensTrayLoader::homeWorker(bool moveToReadyPosAfterHome)
{
    kickerAndClampToReady();
    ltLoaderTrackX->axis()->home();
    ltLoaderBeltZ->axis()->home(false);
    ltBufferBeltZ->axis()->home();
    ltLoaderBeltZ->axis()->waitHomeDone();
    if (!moveToReadyPosAfterHome)
    {
        QThread::msleep(50);
        ltLoaderBeltZ->axis()->moveToNegativeLimit();
    }

    if (moveToReadyPosAfterHome)
    {
        ltBufferBeltZ->moveTo(SAxisPosName::LoadTrayPos, false);
        ltLoaderBeltZ->moveTo(SAxisPosName::DownPos, false);
        if (ltLoaderStageSensor->get(-1))
        {
            ltlLeftDam->set(true);
            ltlClamp->set(true);
            moveToPrPosImpl(0);
        }
        else
        {
            ltLoaderTrackX->moveTo(SAxisPosName::LoadTrayPos);
        }
        ltBufferBeltZ->waitArrivedPos(SAxisPosName::LoadTrayPos);
        ltLoaderBeltZ->waitArrivedPos(SAxisPosName::DownPos);
    }
}

void LensTrayLoader::loadTray()
{
    checkIsInit();

    if (!hasSynchronizeHasTray)
    {
        synchronizeHasTray();
        if (ltState->lensTrayLoaderHasTray())
        {
            verifyLensTrayMap();
            ltlLeftDam->set(true);
            ltlClamp->set(true);
        }
        hasSynchronizeHasTray = true;
    }
    if (!ltState->lensTrayLoaderHasTray())
    {
        if (isLtBufferStopped)
        {
            handleStop();
            return;
        }
        ltBuffer->pushEvent(WorkerEvent("getNewTray"));

        loadTrayImpl();

        if (isLtBufferStopped)
        {
            handleStop();
            return;
        }
        ltBuffer->pushEvent(WorkerEvent("getNewTray"));
    }
}

void LensTrayLoader::unloadTray()
{
    checkIsInit();
    if (ltState->lensTrayLoaderHasTray())
    {
        unloadTrayImpl();
    }
}

void LensTrayLoader::kickerAndClampToReady()
{
    checkIsInit();

    ltlUnloadKicker->set(false, false);
    ltlClamp->set(false, false);
    if (ltlLeftDam->get() != SCCylinder::True)
    {
        QThread::msleep(50);
        ltlLeftDam->set(true);
    }
    ltlUnloadKicker->wait(false);
    ltlClamp->wait(false);
}

void LensTrayLoader::ltMagArrivedFirstInUseTrayPos()
{
    isLtMagArrivedFirstInUseTrayPos = true;
}

void LensTrayLoader::getLens()
{
    checkIsInit();

    if (!ltState->lensTrayLoaderHasTray())
    {
        pushEvent(WorkerEvent("loadTray"));
        pushEvent(WorkerEvent("getLens"));
        return;
    }

    for (int columnIndex = currentColumnIndex; columnIndex < lensTrayConfig->columns(); columnIndex++)
    {
        currentColumnIndex = columnIndex;
        for (int rowIndex = 0; rowIndex < lensTrayConfig->rows(); rowIndex++)
        {
            if (isLtBufferStopped)
            {
                handleStop();
                return;
            }

            auto lensData = lensTrayMap->getData<LensTrayMaterialData>(rowIndex, columnIndex);
            if (lensData->materialState() == LensTrayMaterialData::PrOk)
            {
                m_gotLens = true;
                return;
            }
            if (lensData->materialState() == LensTrayMaterialData::Init)
            {
                moveToPrPosImpl(columnIndex);
                PrOffset prOffset;
                if (ltLensLocation->performPR(prOffset, 0, rowIndex))
                {
                    correctPrOffset(prOffset);
                    lastLensOffset = prOffset;
                    lastLensIndex.setX(rowIndex);
                    lastLensIndex.setY(columnIndex);
                    lensData->setMaterialState(LensTrayMaterialData::PrOk);
                    m_gotLens = true;
                    return;
                }
                else
                {
                    lensData->setMaterialState(LensTrayMaterialData::PrNg);
                    continue;
                }
            }
        }
    }

    // have to change tray first
    pushEvent(WorkerEvent("unloadTray"));
    pushEvent(WorkerEvent("loadTray"));
    pushEvent(WorkerEvent("getLens"));
}

void LensTrayLoader::ltBufferStopped()
{
    isLtBufferStopped = true;
    if (!isRunning())
    {
        handleStop();
    }
}

void LensTrayLoader::lensTrayBufferManuallyLoadTray()
{
    checkIsInit();

    synchronizeHasTray();
    ltBuffer->manuallyGetNewTray();
}

void LensTrayLoader::manuallyLoadTray()
{
    checkIsInit();

    synchronizeHasTray();
    if (ltState->lensTrayLoaderHasTray())
    {
        UIOperation::getIns()->showTip(tr("Lens tray loader already detected tray!"));
        return;
    }
    QVariantList args;
    args << false;
    ltBuffer->pushEvent(WorkerEvent("manuallyGetNewTray", args));
    loadTrayImpl();
}

void LensTrayLoader::manuallyUnloadTray()
{
    checkIsInit();

    synchronizeHasTray();
    if (!ltState->lensTrayLoaderHasTray())
    {
        UIOperation::getIns()->showTip(tr("Lens tray loader did not detect tray!"));
        return;
    }
    unloadTrayImpl();
}

void LensTrayLoader::synchronizeHasTray()
{
    ltState->setLensTrayLoaderHasTray(ltLoaderStageSensor->get(-1));
}

void LensTrayLoader::startLoadUnloadTrayLoopTest(int times)
{
    checkIsInit();

    kickerAndClampToReady();
    synchronizeHasTray();
    ltBuffer->synchronizeHasTray();

    ltMag->setInLoadUnloadTrayLoopTestMode(true);

    WorkerEvent loadTrayEvent("loadTray");
    WorkerEvent doDummyWorkEvent("doDummyWork");
    WorkerEvent unloadTrayEvent("unloadTray");
    for (int i = 0; i < times; i++)
    {
        pushEvent(loadTrayEvent);
        pushEvent(doDummyWorkEvent);
        pushEvent(unloadTrayEvent);
    }
    pushEvent(WorkerEvent("loadUnloadTrayLoopTestFinished"));
}

void LensTrayLoader::doDummyWork()
{
    for (int i = 0; i < lensTrayConfig->columns(); i++)
    {
        //        int realColumnIndex = lensTrayConfig->columns() - 1 - i;
        int realColumnIndex = i;
        moveToPrPosImpl(realColumnIndex);
        ltLensLocation->getImage();
    }
}

void LensTrayLoader::stopLoadUnloadTrayLoopTest()
{
    ltMag->setInLoadUnloadTrayLoopTestMode(false);

    WorkerEvent abortEvent(SMD::Abort);
    pushEvent(abortEvent);
    emit postEvent(WorkerName::LensTrayBuffer, abortEvent);
    emit postEvent(WorkerName::LensTrayMagazine, abortEvent);
}

void LensTrayLoader::resetLoadUnloadTrayLoopTest()
{
    WorkerEvent resetEvent(SMD::Reset);
    pushEvent(resetEvent);
    emit postEvent(WorkerName::LensTrayBuffer, resetEvent);
    emit postEvent(WorkerName::LensTrayMagazine, resetEvent);
}

void LensTrayLoader::loadUnloadTrayLoopTestFinished()
{
    ltMag->setInLoadUnloadTrayLoopTestMode(false);
}

void LensTrayLoader::moveToPrPos(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    lensTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    moveToPrPosImpl(unitColumnIndex, columnIndex);
}

void LensTrayLoader::moveToPerformLensPr(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    lensTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    moveToPrPosImpl(unitColumnIndex, columnIndex);
    getLensPrOffset(unitRowIndex * lensTrayConfig->rowsInsideUnit() + rowIndex);
}

void LensTrayLoader::moveToPerformVacancyPr(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    lensTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    moveToPrPosImpl(unitColumnIndex, columnIndex);
    getLTVacancyPrOffset(unitRowIndex * lensTrayConfig->rowsInsideUnit() + rowIndex);
}

void LensTrayLoader::loadTrayImpl()
{
    ltlLeftDam->set(true, false);
    ltlClamp->set(false, false);
    ltLoaderTrackX->moveTo(SAxisPosName::LoadTrayPos, false);
    ltLoaderBeltZ->moveTo(SAxisPosName::DownPos, false);
    ltLoaderTrackX->waitArrivedPos(SAxisPosName::LoadTrayPos);
    ltlClamp->wait(false);
    ltlLeftDam->wait(true);

    QElapsedTimer timer;
    timer.start();
    while (true)
    {
        if (ltState->lensTrayBufferHasTray())
        {
            break;
        }
        if (hasAbortEvent())
        {
            return;
        }
        if (isLtBufferStopped)
        {
            handleStop();
            return;
        }
        if (timer.elapsed() > 20000)
        {
            timer.restart();
            qCWarning(logCate()) << tr("Waiting lens tray buffer has tray...");
        }
        QThread::msleep(10);
    }

    double ltBufferBeltZUnloadTrayPos = ltBufferBeltZ->getPos<SingleAxisPos>(SAxisPosName::UnloadTrayPos)->pos();
    ltBufferBeltZ->axis()->tryToMove(ltBufferBeltZUnloadTrayPos);
    ltLoaderBeltZ->waitArrivedPos(SAxisPosName::DownPos);
    ltlClamp->set(true);
    ltLoaderTrackX->moveToWithOffset(SAxisPosName::SafetyPosForLTBufferZ, 0.1);
    ltBufferBeltZ->moveTo(SAxisPosName::SafetyPosUponLTL);
    ltLoaderBeltZ->moveTo(SAxisPosName::StandbyPos, false);
    ltState->setLensTrayBufferHasTray(false);
    ltState->setLensTrayLoaderHasTray(true);
    resetRowColumnIndex();
    lensTrayMap->reset();
}

void LensTrayLoader::verifyLensTrayMap()
{
    for (int i = 0; i < lensTrayConfig->rows(); i++)
    {
        for (int j = 0; j < lensTrayConfig->columns(); j++)
        {
            auto data = lensTrayMap->getData<LensTrayMaterialData>(i, j);
            if (data->materialState() == LensTrayMaterialData::PrOk)
            {
                data->setMaterialState(LensTrayMaterialData::Init);
            }
        }
    }
}

void LensTrayLoader::unloadTrayImpl()
{
    QElapsedTimer timer;
    timer.start();
    while (true)
    {
        if (!ltBuffer->isGettingNewTray())
        {
            break;
        }
        if (timer.elapsed() > 20000)
        {
            qCWarning(logCate()) << tr("Waiting lens tray buffer getting tray...");
            timer.restart();
        }
        QThread::msleep(10);
    }

    if (!ltBufferBeltZ->greaterThanPos(SAxisPosName::SafetyPosUponLTL))
    {
        if (ltLoaderTrackX->lessThanPos(SAxisPosName::SafetyPosForLTBufferZ))
        {
            ltLoaderTrackX->moveToWithOffset(SAxisPosName::SafetyPosForLTBufferZ, 0.1);
        }
        ltBufferBeltZ->moveToWithOffset(SAxisPosName::SafetyPosUponLTL, 0.1);
    }

    isLtMagArrivedFirstInUseTrayPos = false;
    QVariantList args;
    args << true;
    ltMag->pushEvent(WorkerEvent("moveToFirstInUseTrayPos", args));
    ltLoaderTrackX->moveTo(SAxisPosName::UnloadTrayPos);
    ltlClamp->set(false, false);
    QThread::msleep(50);
    ltlLeftDam->set(false, false);
    ltlClamp->wait(false);
    ltLoaderBeltZ->moveTo(SAxisPosName::UpPos);
    ltlLeftDam->wait(false);

    timer.restart();
    while (true)
    {
        if (hasAbortEvent())
        {
            return;
        }
        if (isLtMagArrivedFirstInUseTrayPos)
        {
            break;
        }
        if (isLtBufferStopped)
        {
            handleStop();
            return;
        }
        if (timer.elapsed() > 20000)
        {
            qCWarning(logCate()) << tr("Waiting ltMag ArrivedFirstInUseTrayPos...");
            timer.restart();
        }
        QThread::msleep(10);
    }

    ltLoaderBelt->velocityMove(beltRunDir);
    ErrorHandler::tryToHandleGeneralError<void>(
        [this] {
            QElapsedTimer timer;
            timer.start();
            while (true)
            {
                if (timer.elapsed() > ltConfig->ltLoaderPushOutTrayTimeout())
                {
                    ltLoaderBelt->stop();
                    throw GeneralError(workerName(), tr("Lens tray loader pushout tray timeout!"));
                }
                if (!ltLoaderStageSensor->get(-1))
                {
                    ltLoaderBelt->stop();
                    break;
                }
                QThread::msleep(10);
            }
        },
        [this] { ltLoaderBelt->velocityMove(beltRunDir); });

    while (true)
    {
        ltlUnloadKicker->set(true);
        ltlUnloadKicker->set(false);
        try
        {
            ErrorHandler::tryToHandleError([this] {
                if (ltLoaderLeftSensor->get(-1))
                {
                    throw KickTrayError(WorkerName::LensTrayLoader,
                                        tr("LensTrayLoader推出Tray, "
                                           "但ltLoaderLeftSensor检测到仍然有Tray!\r\n选项解释:\r\nRetry: 重新检测  "
                                           "ReKick: 重新推Tray"));
                }
            });
            break;
        }
        catch (UnhandledError &ue)
        {
            ue.what();
            continue;    // ReKick
        }
    }

    ltMag->setCurrentTrayState(MagTrayState::EmptyTray);
    ltState->setLensTrayLoaderHasTray(false);
    ltMag->setIsLoadingUnloadingTray(false);
}

void LensTrayLoader::moveToPrPosImpl(int unitColumnIndex, int columnIndex)
{
    SingleAxisPos *leftLensPrPos = ltLoaderTrackX->getPos<SingleAxisPos>(SAxisPosName::LeftLensPrPos);
    double delta = unitColumnIndex * lensTrayConfig->unitColumnDelta() + columnIndex * lensTrayConfig->columnDelta();
    double targetPos = leftLensPrPos->pos() - delta;
    ltLoaderTrackX->axis()->absMove(targetPos, true, leftLensPrPos->precision());
}

void LensTrayLoader::moveToPrPosImpl(int columnIndex)
{
    int unitColumnIndex = columnIndex / lensTrayConfig->columnsInsideUnit();
    int columnIndexInsideUnit = columnIndex % lensTrayConfig->columnsInsideUnit();
    moveToPrPosImpl(unitColumnIndex, columnIndexInsideUnit);
}

void LensTrayLoader::resetRowColumnIndex()
{
    currentColumnIndex = 0;
}

void LensTrayLoader::handleStop()
{
    clearEvent();
    stopEventHandled();
}
